/*
 * LagrangeInterpolator.cpp
 *
 *  Created on: 24 Aug 2011
 *      Author: Allan
 */

#include "LagrangeInterpolator.h"

LagrangeInterpolator::LagrangeInterpolator() 
{}

LagrangeInterpolator::LagrangeInterpolator(const vector<double>& xi, const vector<double>& yi, unsigned order) :
xi(xi), yi(yi), order(order)
{}

const double LagrangeInterpolator::operator()(double x) const
{
	// The number of interpolation points
	const unsigned N = xi.size();
	
	// Check if x is less than the smallest interpolation x-point
	if(x <= xi[0]) return yi[0];
	
	// Check if x is larger than the largest interpolation x-point
	if(x >= xi[N-1]) return yi[N-1];
	
	// Otherwise, find the interval where x lives
	unsigned pos, npoints; FindPosition(x, pos, npoints);
	
	return LagrangeInterpolation(x, &xi[pos], &yi[pos], npoints);
}

void LagrangeInterpolator::FindPosition(double x, unsigned& pos, unsigned& npoints) const
{
	// The number of interpolation points
	const unsigned N = xi.size();
	
	for(pos = 0; pos < N-order; pos += order)
		if(xi[pos] <= x && x <= xi[pos+order]) 
			{ npoints = order + 1; return; }
	
	npoints = N - pos;
}

const double LagrangeInterpolator::LagrangeInterpolation(double x, const double* xpoints, const double* ypoints, unsigned npoints) const
{
	double y = 0.0;

	for(unsigned i = 0; i < npoints; ++i)
	{
		double lagrangei = 1.0;

		for(unsigned j = 0; j < npoints; ++j)
			lagrangei *= (i == j) ? 1.0 : (x - xpoints[j])/(xpoints[i] - xpoints[j]);

		y += ypoints[i] * lagrangei;
	}

	return y;
}
